/*
  File: driver.cpp
  Original author: HANGZHOU OLE-SYSTEMS
  Date of creation:
  Modifier: TAN Hongkui 谭洪奎
  Email of modifier: tanhongkui@qq.com
  Date of modification: September 8, 2023
  Description: This file contains the main() function and class Decoder that can parse packets of LiDAR and re-pack them according to protocol simple of ROKIT Locator.
*/

// #include "constants.h"
#include <chrono>
#include <Poco/BinaryWriter.h>
// #include <numbers>
// #include <spdlog/spdlog.h>
// #include <iostream>
// #include <sstream>

// #include <dynamic_reconfigure/server.h>

// #include <sensor_msgs/LaserScan.h>

// #include "oleiPacket.h"
// #include "oleiScan.h"
// #include "oleiPuckConfig.h"
#include "driver.cpp"
// here maskoff waring of macro 'ROS_LOG'
#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"

namespace olelidar
{

  // using namespace sensor_msgs;
  using namespace olei_msgs;

  class Decoder
  {
  public:
    explicit Decoder(std::filesystem::path config_path);

    Decoder(const Decoder &) = delete;
    Decoder operator=(const Decoder &) = delete;

    void PacketCb(const oleiPacketConstPtr &packet_msg);
    // void ConfigCb(oleiPuckConfig &config, int level);
    void PublishMsg(BinaryInterfaceServer *server, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, std::chrono::steady_clock::time_point t);
    std::string serialize_datagram(LaserMessage &simple_datagram);
    bool serialize_and_send_datagram_poco(LaserMessage &simple_datagram);

    bool debug_;

  private:
    /// 9.3.1.3 Data Point
    /// A data point is a measurement by one laser channel of a relection of a laser pulse
    struct DataPoint
    {
      uint16_t azimuth;      // unit: 0.01 degree
      uint16_t distance;     // unit: 1mm
      uint16_t reflectivity; // unit: 1 percent,100 is the reflet of white target
      uint16_t distance2;    // rsv for dual return
    } __attribute__((packed));
    static_assert(sizeof(DataPoint) == 8, "sizeof(DataPoint) != 8");

    struct FiringSequence
    {
      DataPoint points[kFiringsPerSequence]; // 8
    } __attribute__((packed));
    static_assert(sizeof(FiringSequence) == 8, "sizeof(FiringSequence) != 8");

    struct DataHead
    {
      uint8_t magic[4];
      uint8_t version[2];
      uint8_t scale;
      uint8_t oem[3];
      uint8_t model[12];
      uint8_t code[2];
      uint8_t hw[2];
      uint8_t sw[2];
      uint32_t timestamp;
      uint16_t rpm;
      uint8_t flag[2];
      uint32_t rsv;
    } __attribute__((packed));
    static_assert(sizeof(DataHead) == 40, "sizeof(DataBlock) != 40");

    struct DataBlock
    {
      FiringSequence sequences[kSequencesPerBlock]; // 8 * 1
    } __attribute__((packed));
    static_assert(sizeof(DataBlock) == 8, "sizeof(DataBlock) != 8");

    /// 9.3.1.5 Packet format for 2d
    struct Packet
    {
      DataHead head;
      DataBlock blocks[kBlocksPerPacket]; // 8 * 150

    } __attribute__((packed));
    static_assert(sizeof(Packet) == 1240, "sizeof(DataBlock) != 1240");
    static_assert(sizeof(Packet) == sizeof(olei_msgs::oleiPacket().data), "sizeof(Packet) != 1240");

    void DecodeAndFill(const Packet *const packet_buf, uint64_t);

  private:
    bool CheckFactoryBytes(const Packet *const packet);

    // ROS related parameters
    uint32_t laststamp;
    uint32_t scantime;
    std::chrono::steady_clock::time_point start;
    bool is_time_base_{false};
    std::chrono::steady_clock::time_point local_timestamp_base_;
    uint32_t inner_timestamp_base_;

    // config
    // std::string frame_id_;
    float range_min_mm{200.f}; // millimeter
    float range_max_mm;        // millimeter
    float range_min_;          // meter
    float range_max_;          // meter
    float angle_start_;
    float angle_end_;
    bool inverted_;
    float step_;
    int poly_ = 1;

    // load the YAML configuration file
    YAML::Node config_yaml;

    // add vector for laserscan
    std::vector<uint16_t> scanAngleVec_;
    std::vector<uint16_t> scanRangeVec_;
    std::vector<uint16_t> scanIntensityVec_;

    std::vector<uint16_t> scanAngleInVec_;
    std::vector<uint16_t> scanRangeInVec_;
    std::vector<uint16_t> scanIntensityInVec_;

    std::vector<float> scanRangeBuffer;
    std::vector<float> scanIntensityBuffer;
    uint16_t azimuthLast_;
    uint16_t azimuthNow_;
    uint16_t azimuthFirst_;

    // laserscan msg
    uint32_t scanMsgSeq_;
    // 电机频率
    float frequency{0.f};
    int previous_rpm{0};
    int current_rpm{0};
    // 雷达型号类型
    unsigned char lidarType = 0x01;
    // 电机方向定义
    int direction;

    std::chrono::steady_clock::time_point log_time_ = std::chrono::steady_clock::now();

    std::shared_ptr<Driver> drv_;

    // Protocol simple related variables
    int simple_port_{4242};
    BinaryInterfaceServer *server;
    LaserMessage simple_datagram;
  };

  // Decoder::Decoder(const ros::NodeHandle &pnh)
  Decoder::Decoder(std::filesystem::path config_path)
  {
    config_yaml = YAML::LoadFile(config_path);

    angle_start_ = config_yaml["angle_min"].as<float>();
    angle_end_ = config_yaml["angle_max"].as<float>();
    range_min_ = config_yaml["range_min"].as<float>(); // meter
    range_max_ = config_yaml["range_max"].as<float>(); // meter
    inverted_ = config_yaml["inverted"].as<bool>();
    debug_ = config_yaml["debug"].as<bool>();

    // frame_id_ = "olelidar";
    range_max_mm = range_max_ * 1000;
    spdlog::info("===========================");
    // spdlog::info("Frame_id: {}", frame_id_.c_str());
    // spdlog::info("Topic: /{}/scan", frame_id_.c_str());
    spdlog::info("Range: [{} ~ {}] mm", range_min_mm, range_max_mm);
    spdlog::info("===========================");

    start = std::chrono::steady_clock::now();
    // dynamic callback
    // cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));
    // packet receive
    azimuthLast_ = 0;
    azimuthNow_ = 0;
    azimuthFirst_ = 0xFFFF;
    // laserscan msg init
    scanMsgSeq_ = 0;
    direction = 0;

    drv_ = std::make_shared<Driver>(config_path);

    // scan_pub_ = pnh_.advertise<LaserScan>("scan", 100);
    server = new BinaryInterfaceServer(simple_port_);

#ifdef DRIVER_MODULE
    packet_sub_ = pnh_.subscribe<oleiPacket>("packet", 100, &Decoder::PacketCb, this, ros::TransportHints().tcpNoDelay(true));
#endif
    drv_->setCallback(std::bind(&Decoder::PacketCb, this, std::placeholders::_1));

    spdlog::info("Drive Ver:2.0.11");
    spdlog::info("Decoder initialized");

    // TODO
    simple_datagram.angleStart = deg2rad(angle_start_);
    simple_datagram.angleEnd = deg2rad(angle_end_);

    simple_datagram.minRange = range_min_;
    simple_datagram.maxRange = range_max_;

    simple_datagram.hasIntensities = true;
    simple_datagram.minIntensity = 0.f;
    simple_datagram.maxIntensity = 1000.f;
  }

  void Decoder::PublishMsg(BinaryInterfaceServer *server, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, std::chrono::steady_clock::time_point lidar_time)
  {
    int min = angle_start_ * 100 + 18000;
    int max = angle_end_ * 100 + 18000;

    scanRangeBuffer.clear();
    scanIntensityBuffer.clear();

    for (uint16_t i = 0; i < scanAngleInVec_.size(); i++)
    {
      // 过滤出指定角度范围内点云
      int angle = scanAngleInVec_[i];
      if (angle >= min && angle <= max && i % poly_ == 0)
      {
        float range = scanRangeInVec_[i] * 0.001f;
        float intensities = scanIntensityInVec_[i] * 1.0f;

        scanRangeBuffer.push_back(range);
        scanIntensityBuffer.push_back(intensities);
      }
    }
    uint32_t bufferlen = scanRangeBuffer.size();
    simple_datagram.ranges.resize(bufferlen);
    simple_datagram.intensities.resize(bufferlen);

    if (direction == 0) // 电机旋转顺时针时，转换右手坐标系法则
    {
      reverse(scanRangeBuffer.begin(), scanRangeBuffer.end());
      reverse(scanIntensityBuffer.begin(), scanIntensityBuffer.end());
    }
    // std::cout << "Ranges and intensities: \n";
    for (uint16_t i = 0; i < bufferlen; i++)
    {
      simple_datagram.ranges[i] = scanRangeBuffer[i];
      simple_datagram.intensities[i] = scanIntensityBuffer[i];
      // std::cout << simple_datagram.ranges[i] << ", ";
      // std::cout << simple_datagram.intensities[i] << "; ";
    }
    // std::cout << "\n";
    simple_datagram.rangeArraySize = simple_datagram.ranges.size();
    simple_datagram.intensityArraySize = simple_datagram.intensities.size();
    simple_datagram.numBeams = simple_datagram.rangeArraySize;
    uint32_t &len = simple_datagram.numBeams;
    // 扫描顺序自增ID序列
    simple_datagram.scanNum = scanMsgSeq_;
    simple_datagram.uniqueId = scanMsgSeq_;
    scanMsgSeq_++;
    // 激光数据时间戳
    // simple_datagram.header.stamp = lidar_time;
    simple_datagram.time_start = (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch())).count();

    float step = (angle_end_ - angle_start_) / (len - 1);
    // 角度分辨率
    simple_datagram.angleInc = deg2rad(step);
    // 扫描的时间间隔
    simple_datagram.duration_rotate = 1 / frequency;
    simple_datagram.duration_scan = simple_datagram.duration_rotate * (angle_end_ - angle_start_) / 360;
    // 时间分辨率（相邻两个角度之间耗费时间）
    simple_datagram.duration_beam = simple_datagram.duration_scan / len;
    // 修正为雷达内部时钟间隔
    // simple_datagram.scan_time = scantime*0.001f;		//雷达时间戳为毫秒计数，故而转为秒单位应乘以0.001
    // simple_datagram.time_increment = simple_datagram.scan_time/float(len);

    spdlog::info("beam count: {} \tangle increment: {:.3f} deg", len, step);
    spdlog::info("scan frequency: {:.0f} Hz \tbeam duration: {:.3e} s", frequency, simple_datagram.duration_beam);
    serialize_and_send_datagram_poco(simple_datagram);

    // TODO
    // uint16_t size = scanAngleInVec_.size();
    // uint16_t fb = 360 / (step_);
    // if (fb == size - 1)
    // // if (fb == size)
    // {
    //   // 校验当符合点数完整的一帧数据才向外发布话题

    //   // std::string buffer = serialize_datagram(simple_datagram);
    //   // server->write(&buffer, sizeof(buffer));
    //   serialize_and_send_datagram_poco(simple_datagram);
    // }
    // else
    // {
    //   if (scanMsgSeq_ == 1)
    //     return;
    //   spdlog::info("pointCloud frame:{}  size:{}  scanMsgSeq_:{}", fb, size, scanMsgSeq_);
    // }
  }

  std::string Decoder::serialize_datagram(LaserMessage &simple_datagram)
  {
    std::ostringstream oss("", std::ios_base::ate);

    oss << simple_datagram.scanNum;
    oss << simple_datagram.time_start;
    oss << simple_datagram.uniqueId;
    oss << simple_datagram.duration_beam;
    oss << simple_datagram.duration_scan;
    oss << simple_datagram.duration_rotate;
    oss << simple_datagram.numBeams;
    oss << simple_datagram.angleStart;
    oss << simple_datagram.angleEnd;
    oss << simple_datagram.angleInc;
    oss << simple_datagram.minRange;
    oss << simple_datagram.maxRange;

    oss << simple_datagram.rangeArraySize;

    std::cout << "\nRanges:\n";
    for (const auto range : simple_datagram.ranges)
    {
      oss << (std::isnan(range) ? -1e4f : range);
      std::cout << range << ", ";
    }

    oss << static_cast<char>(simple_datagram.hasIntensities);
    oss << simple_datagram.minIntensity;
    oss << simple_datagram.maxIntensity;

    oss << simple_datagram.intensityArraySize;

    std::cout << "\nIntensities:\n";
    for (const auto intensity : simple_datagram.intensities)
    {
      oss << intensity;
      std::cout << intensity << ", ";
    }
    // TODO Return a variable by reference
    return oss.str();
  }

  bool Decoder::serialize_and_send_datagram_poco(LaserMessage &simple_datagram)
  {
    const size_t resulting_msg_size = 2                                         // scanNum
                                      + 5 * 8                                   // time_start, uniqueId, duration_beam, duration_scan, duration_rotate
                                      + 6 * 4                                   // numBeams, angleStart, angleEnd, angleInc, minRange, maxRange
                                      + 4                                       // ranges->length
                                      + simple_datagram.rangeArraySize * 4      // ranges->elements
                                      + 1                                       // hasIntensities
                                      + 2 * 4                                   // minIntensity, maxIntensity
                                      + 4                                       // intensities->length
                                      + simple_datagram.intensityArraySize * 4; // intensities->elements
    Poco::Buffer<char> buffer(resulting_msg_size);
    Poco::MemoryBinaryWriter writer(buffer, Poco::BinaryWriter::StreamByteOrder::LITTLE_ENDIAN_BYTE_ORDER);

    writer << simple_datagram.scanNum;
    writer << simple_datagram.time_start;
    writer << simple_datagram.uniqueId;
    writer << simple_datagram.duration_beam;
    writer << simple_datagram.duration_scan;
    writer << simple_datagram.duration_rotate;
    writer << simple_datagram.numBeams;
    writer << simple_datagram.angleStart;
    writer << simple_datagram.angleEnd;
    writer << simple_datagram.angleInc;
    writer << simple_datagram.minRange;
    writer << simple_datagram.maxRange;

    // writer.writeRaw(simple_datagram.ranges, simple_datagram.ranges.size() * sizeof(float));
    writer << simple_datagram.rangeArraySize;

    if (debug_)
      std::cout << "\nRanges:\n";
    for (const auto range : simple_datagram.ranges)
    {
      writer << (std::isnan(range) ? -1e4f : range);
      if (debug_)
        std::cout << range << ", ";
    }

    writer << static_cast<char>(simple_datagram.hasIntensities);
    writer << simple_datagram.minIntensity;
    writer << simple_datagram.maxIntensity;
    writer << simple_datagram.intensityArraySize;

    if (debug_)
      std::cout << "\nIntensities:\n";
    for (const auto intensity : simple_datagram.intensities)
    {
      writer << intensity;
      if (debug_)
        std::cout << intensity << ", ";
    }

    writer.flush();
    if (resulting_msg_size != buffer.size())
    {
      std::cerr << "Message sizes mismatch!" << std::endl;
      return false;
    }

    // server->write(&buffer, sizeof(buffer));
    server->write(buffer.begin(), buffer.size());
    // TODO Return a variable by reference
    return true;
  }

  void Decoder::DecodeAndFill(const Packet *const packet_buf, uint64_t time)
  {

    // For each data block, 150 total
    uint16_t azimuth;
    uint16_t range; // mm
    uint16_t intensity;
    for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk)
    {
      const auto &block = packet_buf->blocks[iblk];

      // simple loop
      azimuth = block.sequences[0].points[0].azimuth;
      range = block.sequences[0].points[0].distance;
      intensity = block.sequences[0].points[0].reflectivity;
      // 排除异常点云数据
      if (range > range_max_mm || range < range_min_mm)
      {
        range = 0;
        intensity = 0;
      }
      // azimuth ge 36000 is not valid
      if (azimuth < 0xFF00)
      {
        scanAngleVec_.push_back(azimuth);
        scanRangeVec_.push_back(range);
        scanIntensityVec_.push_back(intensity);
      }
    }
  }

  void Decoder::PacketCb(const oleiPacketConstPtr &packet_msg)
  {
    std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
    std::chrono::duration delta_t = now - log_time_;
    log_time_ = now;

    const auto *packet_buf = reinterpret_cast<const Packet *>(&(packet_msg->data[0]));
    azimuthNow_ = packet_buf->blocks[0].sequences[0].points[0].azimuth;
    // 取得第一个数据包
    if (azimuthFirst_ == 0xFFFF)
    {
      // 雷达型号类型
      lidarType = packet_buf->head.code[1];
      azimuthFirst_ = azimuthNow_;
      // 取得转速
      int rpm = (packet_buf->head.rpm) & 0x7FFF;
      // 取得电机旋转方向
      direction = (packet_buf->head.rpm) >> 15;
      spdlog::info("rpm:{}  direction:{}  lidarType:{}", rpm, direction, lidarType);
      // 是否启用倒装设定
      if (inverted_)
        direction = !direction;
    }

    if (azimuthLast_ < azimuthNow_)
    {
      DecodeAndFill(packet_buf, std::chrono::duration_cast<std::chrono::nanoseconds>(packet_msg->stamp.time_since_epoch()).count());
      azimuthLast_ = azimuthNow_;
      return;
    }
    else
    {
      azimuthLast_ = azimuthNow_;
    }
    // scan first half route
    if (azimuthFirst_ >= 200)
    {
      azimuthFirst_ = azimuthNow_;
      return;
    }
    // 雷达时间戳
    uint32_t nowstamp = packet_buf->head.timestamp;
    std::chrono::steady_clock::time_point lidar_time = packet_msg->stamp;
    // uint64_t lidar_time = packet_msg->stamp;

#ifdef TIMESTAMP_DEBUG
    if (!is_time_base_)
    {
      local_timestamp_base_ = ros::Time::now();
      inner_timestamp_base_ = nowstamp;
      lidar_time = local_timestamp_base_;
      is_time_base_ = true;
    }
    else
    {
      uint32_t delta_time = nowstamp - inner_timestamp_base_;
      std::chrono::duration dur_time;
      std::chrono::duration delta_t = dur_time.fromNSec(delta_time * 1000000);
      lidar_time = local_timestamp_base_ + delta_t;
      // ROS_INFO_STREAM("ros timestamp:" << delta_time << "," << delta_t << "," << lidar_time);
    }
#endif
    scantime = nowstamp - laststamp;
    laststamp = nowstamp;
    spdlog::debug("uint32_t, inner current timestamp: {}, scan time: {}", nowstamp, scantime);

    // 雷达工作频率
    current_rpm = (packet_buf->head.rpm) & 0x7FFF;
    if (frequency < 0.001 || current_rpm != previous_rpm)
    {
      previous_rpm = current_rpm;
      lidarType = packet_buf->head.code[1]; // 雷达型号类型
      if (lidarType == 0x01)
      {
        frequency = current_rpm / 60.0;
        step_ = 0.225; // 当雷达型号为0x01类型时，角分辨率为固定值
      }
      else
      {
        if (scanAngleVec_.size() > 2)
        {
          // 角度分辨率
          step_ = (scanAngleVec_[1] - scanAngleVec_[0]) / 100.0;
          frequency = step_ * 10000.0 / 60.0;
        }
        else
        {
          return;
        }
      }
      spdlog::info("frequency: {} hz  step_:{}", frequency, step_);
    }

    // TODO
    // // 当启用NTP服务时，时间戳重定向为NTP服务时间
    // if (packet_buf->head.rsv > 0)
    // {
    //   std::chrono::steady_clock::time_point ntp(packet_buf->head.rsv, packet_buf->head.timestamp);
    //   lidar_time = ntp;
    // }
    scanAngleInVec_.clear();
    scanRangeInVec_.clear();
    scanIntensityInVec_.clear();

    scanAngleInVec_.assign(scanAngleVec_.begin(), scanAngleVec_.end());
    scanRangeInVec_.assign(scanRangeVec_.begin(), scanRangeVec_.end());
    scanIntensityInVec_.assign(scanIntensityVec_.begin(), scanIntensityVec_.end());

    scanAngleVec_.clear();
    scanRangeVec_.clear();
    scanIntensityVec_.clear();

    // 抛出点云数据
    PublishMsg(server, scanRangeInVec_, scanIntensityInVec_, lidar_time);

    // 解码原始数据包
    DecodeAndFill(packet_buf, std::chrono::duration_cast<std::chrono::nanoseconds>(packet_msg->stamp.time_since_epoch()).count());

    // spdlog::info("Time: {}", (ros::Time::now() - start).toSec());
  }
} // namespace olelidar

int main(int argc, char *argv[])
{
  spdlog::info("Usage: simple_ole.AppImage path/config.yaml");
  std::filesystem::path file_path{};
  if (argc == 1)
  {
    spdlog::error("Missing command line argument for the full path to configuration file config.yaml.");
    std::exit(1);
  }
  if (argc > 1)
  {
    file_path = argv[1];
    if (std::filesystem::exists(file_path) && std::filesystem::is_regular_file(file_path))
    {
      spdlog::info("Configuration file: {}", argv[1]);
    }
    else
    {
      spdlog::error("The path to the configuration file config.yaml is not valid.");
      std::exit(1);
    }
  }

  olelidar::Decoder node(file_path);
  if (node.debug_)
    spdlog::set_level(spdlog::level::debug);
  while (true)
  {
    sleep(5);
  }

  return 0;
}
